//
//  RobControl.cpp
//  Robot
//
//  Created by Pei on 15/3/13.
//  Copyright (c) 2015 Pei. All rights reserved.
//


#include <iostream>
#include <stdio.h>
#include <math.h>
#include <iomanip>

#include "RobControl.h"
#include "../udp/Package.h"
#include "../logger/Logger.h"

using namespace std;

//Graden<0 => draait met de klok mee
//Graden>0 => draait tegen de klok in

//richtingsvector van de auto, setter en getter
void RobControl::set_vector (int x, int y) {
    direction_vector_x = x;
    direction_vector_y = y;
}

double RobControl::get_vector_x () {
    return direction_vector_x;
}

double RobControl::get_vector_y () {
    return direction_vector_y;
}

//valideren of de coordinaten geldig zijn, dus geen enkele component -1
bool RobControl::isValidCoords(Info coord){
    if(coord.robx>0 && coord.roby>0 && coord.robhoek>=0 && coord.doelx>0 && coord.doely>0 && coord.garx>0 && coord.gary>0){
        return true;
    }else{
        return false;
    }
}

//berekent de hoek waaronder gedraaid moet  worden
double RobControl::turn_calculate(double rob_x, double rob_y,double rob_theta,double des_x,double des_y) {
    double x, y, ret, val,turn_degree;
    
    val = 180.0 / PI;
    
    x=des_x - rob_x;
    y=des_y - rob_y;
    set_vector(x, y);
    
    ret = atan2 (get_vector_y(),get_vector_x()) * val;

    char buff[100];
    snprintf(buff, 100, "RobControl::turn_calculate (1): (%.2lf, %.2lf) -> %.2lf degrees", get_vector_x(), get_vector_y(), ret);
    Logger::getInstance().log(buff);
    
    if(rob_theta>180)
    {
        rob_theta = rob_theta-360;
    }
    
    turn_degree = ret - rob_theta;
    
    if ( turn_degree>180) {
        turn_degree=turn_degree-360;
    }
    
	string rotateDirection;
    if (turn_degree>0) {
	rotateDirection = "ANTI-CLOCKWISE";
    } else if (turn_degree==0) {
	rotateDirection = "STRAIGHT";
    } else {
	rotateDirection = "CLOCKWISE";
    }
	cout << std::setprecision(2) << "RobControl::turn_calculate (2): The robot rotates " << rotateDirection << " (" << turn_degree << ")\n";
    
    return turn_degree;
    
}

//berekent de afstand tussen de auto en het doel (blik of garage) met de stelling van Pythagoras.
double RobControl::calc_distance(double rob_x, double rob_y,double des_x,double des_y)
{
    double x, y,param,result;
    x=des_x - rob_x;
    y=des_y - rob_y;
    set_vector(x, y);
    
    param=get_vector_x()*get_vector_x() + get_vector_y()*get_vector_y();
    result = sqrt (param);
    printf ("RobControl::calc_distance : %.2f\n", result );
    
    return result;
    
}
/* uitvoering resultaat:
Test: 1:
The rotation degree of direction vector (16.000000,32.000000) is 63.434949 degrees
The robot rotates *ANTI-CLOCKWISE: 0.004949
The robot **travels 35.777088
Test: 2:
The rotation degree of direction vector (12.000000,32.000000) is 69.443955 degrees
The robot rotates *ANTI-CLOCKWISE: 6.013955
The robot **travels 34.176015
Test: 3:
The rotation degree of direction vector (-14.000000,0.000000) is 180.000000 degrees
The robot rotates *CLOCKWISE: -16.000000
The robot **travels 14.000000
Test: 4:
The rotation degree of direction vector (16.000000,0.000000) is 0.000000 degrees
The robot rotates *ANTI-CLOCKWISE: 164.000000
The robot **travels 16.000000
Test: 5:
The rotation degree of direction vector (24.000000,-185.000000) is -82.608316 degrees
The robot rotates *ANTI-CLOCKWISE: 81.391684
The robot **travels 186.550261
Test: 6:
The rotation degree of direction vector (44.000000,35.000000) is 38.500654 degrees
The robot rotates *CLOCKWISE: -157.499346
The robot **travels 56.222771
Program ended with exit code: 0
*/
